#pragma once
#include <Arduino.h>
#include <ArduinoJson.h>
#include <initializer_list>
#include <utility>
#include <math.h>

// 重定向print函数（不变）
inline void toPrint(const String &msg) { Serial.println(msg); }
inline void toPrint(int v) { String s = String(v); Serial.println(s); }

// 引入send.h的函数声明（不变）
bool sendServoControl(std::initializer_list<std::pair<int, int>> items);
bool sendServoControlN(const std::pair<int, int>* items, size_t n);

// ================== 硬件配置（两路HP4067，每路16通道）==================
#define MUX1_S0   16
#define MUX1_S1   17
#define MUX1_S2   18
#define MUX1_S3   19
#define MUX1_EN   21  // 低电平有效
#define MUX1_SIG  33  // ADC引脚

#define MUX2_S0   22
#define MUX2_S1   23
#define MUX2_S2   25
#define MUX2_S3   26
#define MUX2_EN   27  // 低电平有效
#define MUX2_SIG  32  // ADC引脚（独立）

// ================== 通道映射表（仅保留声明，实际发送不再依赖，可删除）==================
const char* CHANNEL_MAP[2][16] = {
  {"00", "01", "02", "03", "04", "05", "06", "07", "08", "09", "10", "11", "12", "13", "14", "15"},
  {"00", "01", "02", "03", "04", "05", "06", "07", "08", "09", "10", "11", "12", "13", "14", "15"}
};

// 舵机脉宽范围（不变）
#define SERVO_PULSE_MIN  100
#define SERVO_PULSE_MAX  599

// ================== 核心修复1：初始化合理的校准参数（匹配电位器实际范围）==================
// 假设电位器输出范围为 100~4000（可根据实际硬件调整，避免与ADC极值重合）
static int  ADC_MIN[2][16] = {
  {100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100}, // MUX1 所有通道最小ADC值
  {100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100}  // MUX2 所有通道最小ADC值
};
static int  ADC_MAX[2][16] = {
  {4000, 4000, 4000, 4000, 4000, 4000, 4000, 4000, 4000, 4000, 4000, 4000, 4000, 4000, 4000, 4000}, // MUX1 最大ADC值
  {4000, 4000, 4000, 4000, 4000, 4000, 4000, 4000, 4000, 4000, 4000, 4000, 4000, 4000, 4000, 4000}  // MUX2 最大ADC值
};
static bool ADC_INVERT[2][16] = {
  {false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, false},
  {false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, false}
};

// ADC配置（不变）
#define ADC_BITS         12
#define ADC_MAX_VALUE    ((1 << ADC_BITS) - 1) // 4095
#define ADC_ATTEN        ADC_11db              // 0~3.3V输入
#define SAMPLE_COUNT     4                     // 采样取平均
#define SETTLE_US        86                    // 通道切换稳定时间
#define MUX_COUNT        2                     // MUX数量
#define CHANNELS_PER_MUX 16                    // 每路MUX通道数
#define TOTAL_CHANNELS   (MUX_COUNT * CHANNELS_PER_MUX) // 总通道数32

// ================== 硬件初始化（不变）==================
inline void initPins() {
  // 初始化MUX1
  pinMode(MUX1_S0, OUTPUT);
  pinMode(MUX1_S1, OUTPUT);
  pinMode(MUX1_S2, OUTPUT);
  pinMode(MUX1_S3, OUTPUT);
  pinMode(MUX1_SIG, INPUT);
#ifdef MUX1_EN
  pinMode(MUX1_EN, OUTPUT);
  digitalWrite(MUX1_EN, LOW);  // 使能MUX1
#endif

  // 初始化MUX2
  pinMode(MUX2_S0, OUTPUT);
  pinMode(MUX2_S1, OUTPUT);
  pinMode(MUX2_S2, OUTPUT);
  pinMode(MUX2_S3, OUTPUT);
  pinMode(MUX2_SIG, INPUT);
#ifdef MUX2_EN
  pinMode(MUX2_EN, OUTPUT);
  digitalWrite(MUX2_EN, LOW);  // 使能MUX2
#endif
}

inline void setupADC() {
  analogSetPinAttenuation(MUX1_SIG, ADC_ATTEN);
  analogSetPinAttenuation(MUX2_SIG, ADC_ATTEN);
  analogReadResolution(ADC_BITS);
}

static bool _mux_adc_inited = false;
inline void ensureMuxADC() {
  if (!_mux_adc_inited) {
    initPins();
    setupADC();
    _mux_adc_inited = true;
  }
}

// ================== MUX通道选择（不变）==================
inline void muxSelect(uint8_t mux_idx, uint8_t ch) {
  if (mux_idx >= MUX_COUNT || ch >= CHANNELS_PER_MUX) {
    Serial.println("[warn] 无效的MUX编号或通道号");
    return;
  }

  if (mux_idx == 0) {  // MUX1
    digitalWrite(MUX1_S0, ch & 0x01);
    digitalWrite(MUX1_S1, (ch >> 1) & 0x01);
    digitalWrite(MUX1_S2, (ch >> 2) & 0x01);
    digitalWrite(MUX1_S3, (ch >> 3) & 0x01);
  } else {  // MUX2
    digitalWrite(MUX2_S0, ch & 0x01);
    digitalWrite(MUX2_S1, (ch >> 1) & 0x01);
    digitalWrite(MUX2_S2, (ch >> 2) & 0x01);
    digitalWrite(MUX2_S3, (ch >> 3) & 0x01);
  }

  digitalWrite(MUX1_EN, (mux_idx == 0) ? LOW : HIGH);
  digitalWrite(MUX2_EN, (mux_idx == 1) ? LOW : HIGH);
}

// ================== 读取ADC通道（不变）==================
inline int readChannel(uint8_t total_ch, uint8_t samples = SAMPLE_COUNT) {
  ensureMuxADC();
  if (total_ch >= TOTAL_CHANNELS) return 0;

  uint8_t mux_idx = total_ch / CHANNELS_PER_MUX;
  uint8_t ch = total_ch % CHANNELS_PER_MUX;

  muxSelect(mux_idx, ch);
  delayMicroseconds(SETTLE_US);
  int adc_pin = (mux_idx == 0) ? MUX1_SIG : MUX2_SIG;
  
  (void)analogRead(adc_pin);  // 丢弃第一次采样

  uint32_t acc = 0;
  for (uint8_t i = 0; i < samples; i++) {
    acc += analogRead(adc_pin);
  }
  int adc_val = (int)(acc / samples);
  // 新增：打印ADC原始值（用于调试校准参数）
  Serial.printf("[DBG] MUX%d-CH%d ADC: %d\n", mux_idx, ch, adc_val);
  return adc_val;
}

// ================== 核心修复2：完善ADC→脉宽转换（增加异常保护）==================
static inline int adcToPulse(int adc, int total_ch) {
  if (total_ch < 0 || total_ch >= TOTAL_CHANNELS) {
    Serial.printf("[warn] 无效总通道号: %d\n", total_ch);
    return SERVO_PULSE_MIN;
  }

  uint8_t mux_idx = total_ch / CHANNELS_PER_MUX;
  uint8_t ch = total_ch % CHANNELS_PER_MUX;

  int  amin = ADC_MIN[mux_idx][ch];
  int  amax = ADC_MAX[mux_idx][ch];
  bool inv  = ADC_INVERT[mux_idx][ch];

  // 修复：若amax <= amin，强制设置默认范围（避免分母为0）
  if (amax <= amin) {
    Serial.printf("[warn] MUX%d-CH%d 校准范围异常，使用默认范围 100~4000\n", mux_idx, ch);
    amin = 100;
    amax = 4000;
  }

  // 计算占空比（确保adc在[amin, amax]范围内）
  int clamped_adc = constrain(adc, amin, amax);
  float t = (float)(clamped_adc - amin) / (amax - amin);
  
  // 信号翻转（如需）
  if (inv) t = 1.0f - t;

  // 计算脉宽（确保在[SERVO_PULSE_MIN, SERVO_PULSE_MAX]范围内）
  int pulse = lroundf(SERVO_PULSE_MIN + t * (SERVO_PULSE_MAX - SERVO_PULSE_MIN));
  pulse = constrain(pulse, SERVO_PULSE_MIN, SERVO_PULSE_MAX);

  // 新增：打印脉宽计算过程（调试用）
  Serial.printf("[DBG] MUX%d-CH%d: ADC=%d → 占空比=%.2f → 脉宽=%d\n", mux_idx, ch, clamped_adc, t, pulse);
  return pulse;
}

// ================== 32路批量采样+发送（不变）==================
inline void readAll32AndSendServos(int pulseValues[32]) {
  std::pair<int, int> buf[32];
  for (int total_ch = 0; total_ch < TOTAL_CHANNELS; ++total_ch) {
    buf[total_ch] = { total_ch, pulseValues[total_ch] };
  }
  sendServoControlN(buf, TOTAL_CHANNELS);
}